########################################################################
#
# Copyright (c) 2022, STEREOLABS.
#
# All rights reserved.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
# A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
# OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
# SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
# DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
# THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#
########################################################################

import os
import sys
import ctypes
import cv2
import numpy as np
import threading
import time
import json
import warnings

# 过滤 PyQt5 弃用警告
warnings.filterwarnings("ignore", category=DeprecationWarning, module="PyQt5")

from PyQt5.QtWidgets import (QApplication, QMainWindow, QWidget, QVBoxLayout, 
                             QHBoxLayout, QLabel, QPushButton, QSlider, QGroupBox,
                             QGridLayout, QMessageBox, QCheckBox, QSpinBox, QLineEdit,
                             QScrollArea, QDialog, QSizePolicy, QComboBox)
from PyQt5.QtCore import QThread, pyqtSignal, Qt, QTimer, QObject
from PyQt5.QtGui import QPixmap, QImage, QMouseEvent

# 在导入 pyzed.sl 之前先加载必要的 DLL 文件
print("正在加载 ZED SDK DLL 文件...")

# 设置 DLL 搜索路径
dll_paths = [
    r"C:\Program Files (x86)\ZED SDK\bin",
    r"C:\Program Files\NVIDIA GPU Computing Toolkit\CUDA\v12.9\bin"
]

# 将路径添加到 DLL 搜索路径
for path in dll_paths:
    if os.path.exists(path):
        os.add_dll_directory(path)
        print(f"  ✓ 添加 DLL 搜索路径: {path}")

# 预加载关键的 DLL 文件
try:
    print("预加载 sl_zed64.dll...")
    sl_zed = ctypes.CDLL(r"C:\Program Files (x86)\ZED SDK\bin\sl_zed64.dll")
    print("  ✓ sl_zed64.dll 加载成功")
except Exception as e:
    print(f"  ✗ sl_zed64.dll 加载失败: {e}")

try:
    print("预加载 sl_ai64.dll...")
    sl_ai = ctypes.CDLL(r"C:\Program Files (x86)\ZED SDK\bin\sl_ai64.dll")
    print("  ✓ sl_ai64.dll 加载成功")
except Exception as e:
    print(f"  ✗ sl_ai64.dll 加载失败: {e}")

print("DLL 加载完成，现在导入 pyzed.sl...")
import pyzed.sl as sl


class BlobDetector:
    """光斑检测器 - 使用轮廓法"""
    
    def __init__(self):
        self.min_area = 5  # 与界面控件默认值一致
        self.max_area = 5000
        self.min_circularity = 0.3
        self.min_convexity = 0.5
        self.threshold_value = 240  # 二值化阈值
    
    def detect_blobs(self, image):
        """使用轮廓法检测光斑"""
        try:
            # 转换为灰度图
            if len(image.shape) == 3:
                gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
            else:
                gray = image
            
            # 应用高斯模糊减少噪声
            blurred = cv2.GaussianBlur(gray, (5, 5), 0)
            
            # 使用固定阈值分割（检测亮斑）
            _, thresh = cv2.threshold(blurred, self.threshold_value, 255, cv2.THRESH_BINARY)
            
            # 形态学操作
            kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (3, 3))
            thresh = cv2.morphologyEx(thresh, cv2.MORPH_CLOSE, kernel)
            thresh = cv2.morphologyEx(thresh, cv2.MORPH_OPEN, kernel)
            
            # 查找轮廓
            contours, _ = cv2.findContours(thresh, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
            
            # 过滤轮廓
            valid_contours = []
            total_contours = len(contours)
            area_filtered = 0
            circularity_filtered = 0
            convexity_filtered = 0
            
            for contour in contours:
                area = cv2.contourArea(contour)
                
                # 面积过滤
                if area < self.min_area or area > self.max_area:
                    area_filtered += 1
                    continue
                
                # 计算圆形度
                perimeter = cv2.arcLength(contour, True)
                if perimeter > 0:
                    circularity = 4 * np.pi * area / (perimeter * perimeter)
                else:
                    circularity = 0
                
                # 圆形度过滤（放宽条件）
                if circularity < 0.1:  # 降低圆形度要求
                    circularity_filtered += 1
                    continue
                
                # 计算凸度
                hull = cv2.convexHull(contour)
                hull_area = cv2.contourArea(hull)
                if hull_area > 0:
                    convexity = area / hull_area
                else:
                    convexity = 0
                
                # 凸度过滤（放宽条件）
                if convexity < 0.3:  # 降低凸度要求
                    convexity_filtered += 1
                    continue
                
                valid_contours.append(contour)
            
            # 添加调试信息（仅在检测到轮廓时打印）
            if total_contours > 0:
                print(f"光斑检测调试 - 总轮廓: {total_contours}, 面积过滤: {area_filtered}, 圆形度过滤: {circularity_filtered}, 凸度过滤: {convexity_filtered}, 有效: {len(valid_contours)}")
                print(f"光斑检测参数 - 最小面积: {self.min_area}, 最大面积: {self.max_area}, 阈值: {self.threshold_value}")
            
            return valid_contours, thresh
            
        except Exception as e:
            print(f"光斑检测异常: {e}")
            return [], None
    
    def draw_blobs(self, image, contours, is_left_camera=True):
        """在图像上绘制光斑检测结果"""
        try:
            result_image = image.copy()
            
            for i, contour in enumerate(contours):
                # 计算质心
                M = cv2.moments(contour)
                if M["m00"] != 0:
                    cx = M["m10"] / M["m00"]
                    cy = M["m01"] / M["m00"]
                else:
                    cx, cy = 0, 0
                
                # 计算轮廓的边界框
                x, y, w, h = cv2.boundingRect(contour)
                size = max(w, h)
                
                # 绘制轮廓
                cv2.drawContours(result_image, [contour], -1, (255, 0, 0), 2)
                
                # 绘制十字线（更细）
                color = (0, 255, 0)  # 绿色
                thickness = 1  # 更细的线条
                length = max(8, size // 3)
                
                # 水平线
                cv2.line(result_image, (int(cx - length), int(cy)), (int(cx + length), int(cy)), color, thickness)
                # 垂直线
                cv2.line(result_image, (int(cx), int(cy - length)), (int(cx), int(cy + length)), color, thickness)
                
                # 绘制质心坐标文本（保留一位小数，增大字体，位置调整避免重叠）
                coord_text = f"({cx:.1f}, {cy:.1f})"
                cv2.putText(result_image, coord_text, (int(cx) + 20, int(cy) - 10), 
                           cv2.FONT_HERSHEY_SIMPLEX, 1.2, color, 3)
                
                # 绘制光斑编号和面积（增大字体，位置调整避免重叠）
                area = cv2.contourArea(contour)
                info_text = f"#{i+1} A:{area:.0f}"
                cv2.putText(result_image, info_text, (int(cx) - 15, int(cy) + 30), 
                           cv2.FONT_HERSHEY_SIMPLEX, 1.0, (255, 255, 0), 3)
                
                # 如果有相机参数，计算并显示三维坐标
                if self.camera_params is not None:
                    # 这里需要左右相机配合计算，暂时只显示2D坐标
                    # 三维坐标计算需要在主程序中完成
                    pass
            
            return result_image
            
        except Exception as e:
            return image


class ThreeDCoordinateThread(QThread):
    """三维坐标计算线程 - 使用OpenCV最优算法计算单个发光标志的三维坐标"""
    
    # 信号定义
    coordinate_ready = pyqtSignal(object)  # 发送单个发光标志的三维坐标结果
    image_update_ready = pyqtSignal(object, object, object)  # 发送左图像、右图像、三维坐标用于更新显示
    
    def __init__(self, camera_params=None):
        super().__init__()
        self.camera_params = camera_params
        self.is_running = False
        self.left_centroids = []
        self.right_centroids = []
        self.left_contours = []
        self.right_contours = []
        self.left_image = None
        self.right_image = None
        self.last_point_3d = None  # 保存上一次的三维坐标，用于防闪烁
        self.last_update_time = 0  # 记录上次更新时间
        self.update_timeout = 2.0  # 2秒超时，确保遮挡移除后能更新
        self.mutex = threading.Lock()
        
        # 如果没有提供相机参数，尝试从JSON文件加载
        if self.camera_params is None:
            self.load_calibration_from_json()
    
    def load_calibration_from_json(self):
        """从JSON文件加载标定参数"""
        try:
            # 获取当前脚本所在目录
            script_dir = os.path.dirname(os.path.abspath(__file__))
            calibration_file = os.path.join(script_dir, "camera_calibration.json")
            
            if os.path.exists(calibration_file):
                with open(calibration_file, 'r', encoding='utf-8') as f:
                    self.camera_params = json.load(f)
                print("✓ 从JSON文件加载标定参数成功")
                print(f"  标定文件: {calibration_file}")
                
                # 显示标定参数信息
                if self.camera_params:
                    calib_info = self.camera_params.get("calibration_info", {})
                    print(f"  棋盘格尺寸: {calib_info.get('chessboard_size', 'Unknown')}")
                    print(f"  方格大小: {calib_info.get('square_size_mm', 'Unknown')} mm")
                    
                    left_cam = self.camera_params.get("left_camera", {})
                    right_cam = self.camera_params.get("right_camera", {})
                    stereo = self.camera_params.get("stereo_camera", {})
                    
                    if left_cam and right_cam and stereo:
                        print(f"  左相机焦距: fx={left_cam.get('camera_matrix', [[0]])[0][0]:.2f}, fy={left_cam.get('camera_matrix', [[0,0,0],[0,0,0],[0,0,1]])[1][1]:.2f}")
                        print(f"  右相机焦距: fx={right_cam.get('camera_matrix', [[0]])[0][0]:.2f}, fy={right_cam.get('camera_matrix', [[0,0,0],[0,0,0],[0,0,1]])[1][1]:.2f}")
                        baseline = abs(stereo.get('translation_vector', [[0]])[0][0])
                        print(f"  基线距离: {baseline:.2f} mm")
            else:
                print(f"✗ 标定文件不存在: {calibration_file}")
                print("  请先运行标定程序生成camera_calibration.json文件")
        except Exception as e:
            print(f"✗ 加载标定参数失败: {e}")
            self.camera_params = None
    
    def update_centroids(self, left_centroids, right_centroids):
        """更新质心坐标"""
        with self.mutex:
            self.left_centroids = left_centroids.copy()
            self.right_centroids = right_centroids.copy()
    
    def update_images(self, left_image, right_image):
        """更新图像数据"""
        with self.mutex:
            self.left_image = left_image.copy() if left_image is not None else None
            self.right_image = right_image.copy() if right_image is not None else None
    
    def update_contours(self, left_contours, right_contours):
        """更新轮廓数据"""
        with self.mutex:
            self.left_contours = left_contours.copy() if left_contours is not None else []
            self.right_contours = right_contours.copy() if right_contours is not None else []
    
    def run(self):
        """线程运行函数"""
        self.is_running = True
        
        while self.is_running:
            try:
                # 获取当前质心坐标和轮廓数据
                with self.mutex:
                    left_centroids = self.left_centroids.copy()
                    right_centroids = self.right_centroids.copy()
                    left_contours = self.left_contours.copy()
                    right_contours = self.right_contours.copy()
                
                # 检查是否有光斑检测结果和相机参数
                if len(left_centroids) > 0 and len(right_centroids) > 0 and self.camera_params is not None:
                    # 选择最佳匹配的光斑对
                    best_left_point, best_right_point = self.find_best_matching_blobs(left_centroids, right_centroids)
                    
                    if best_left_point is not None and best_right_point is not None:
                        # 使用OpenCV前方交会算法计算三维坐标
                        point_3d_opencv = self.calculate_3d_coordinates_opencv(best_left_point, best_right_point, self.camera_params)
                        
                        # 准备结果数据
                        result_data = {
                            'opencv': point_3d_opencv
                        }
                        
                        if point_3d_opencv is not None:
                            # 改进的防闪烁机制：检测光斑重新出现
                            current_time = time.time()
                            should_update = True
                            
                            if self.last_point_3d is not None:
                                # 计算坐标变化
                                diff = np.linalg.norm(np.array(point_3d_opencv) - np.array(self.last_point_3d))
                                time_since_update = current_time - self.last_update_time
                                
                                # 如果坐标变化很小且时间未超时，则不更新
                                if diff < 2.0 and time_since_update < self.update_timeout:
                                    should_update = False
                            
                            if should_update:
                                # 发送结果到界面
                                self.coordinate_ready.emit(result_data)
                                # 保存当前坐标和时间
                                self.last_point_3d = point_3d_opencv.copy()
                                self.last_update_time = current_time
                                print(f"✓ 三维坐标已更新: ({point_3d_opencv[0]:.2f}, {point_3d_opencv[1]:.2f}, {point_3d_opencv[2]:.2f})")
                        else:
                            # 计算失败，发送空结果
                            self.coordinate_ready.emit(result_data)
                            # 清除上一次的坐标，确保下次能正确更新
                            self.last_point_3d = None
                            self.last_update_time = 0
                    else:
                        # 没有找到匹配的光斑对，发送空结果
                        result_data = {
                            'opencv': None
                        }
                        self.coordinate_ready.emit(result_data)
                else:
                    # 没有光斑或相机参数，发送空结果
                    result_data = {
                        'opencv': None
                    }
                    self.coordinate_ready.emit(result_data)
                
                # 休眠一段时间，避免过度计算
                time.sleep(0.1)  # 100ms更新一次，提高响应速度
                
            except Exception as e:
                print(f"三维坐标计算线程错误: {e}")
                time.sleep(0.1)
    
    def find_best_matching_blobs(self, left_centroids, right_centroids):
        """找到最佳匹配的光斑对"""
        try:
            best_left = None
            best_right = None
            min_score = float('inf')
            
            for left_point in left_centroids:
                for right_point in right_centroids:
                    # 计算匹配分数（Y坐标差异应该很小）
                    y_diff = abs(left_point[1] - right_point[1])
                    
                    # 检查X坐标差异（右图像中的点应该在左图像中点的左侧）
                    x_diff = left_point[0] - right_point[0]
                    
                    # 如果Y坐标差异在合理范围内且X坐标差异合理
                    if y_diff < 30 and x_diff > 10 and x_diff < 300:
                        # 计算匹配分数，优先考虑Y坐标匹配
                        score = y_diff * 2 + abs(x_diff - 120) * 0.1  # 假设理想视差约为120像素
                        
                        if score < min_score:
                            min_score = score
                            best_left = left_point
                            best_right = right_point
            
            if best_left is not None and best_right is not None:
                print(f"匹配成功 - 左点: ({best_left[0]:.1f}, {best_left[1]:.1f})")
                print(f"匹配成功 - 右点: ({best_right[0]:.1f}, {best_right[1]:.1f})")
                print(f"匹配成功 - Y差异: {abs(best_left[1] - best_right[1]):.1f} 像素")
                print(f"匹配成功 - X视差: {best_left[0] - best_right[0]:.1f} 像素")
            
            return best_left, best_right
            
        except Exception as e:
            print(f"光斑匹配失败: {e}")
            return None, None
    

    
    def calculate_3d_coordinates_opencv(self, left_point, right_point, camera_params):
        """使用OpenCV前方交会算法计算三维坐标"""
        try:
            if camera_params is None:
                print("相机参数未提供")
                return None
            
            # 提取相机参数
            left_cam = camera_params["left_camera"]
            right_cam = camera_params["right_camera"]
            stereo = camera_params["stereo_camera"]
            
            # 构建相机内参矩阵
            left_matrix = np.array(left_cam["camera_matrix"], dtype=np.float64)
            right_matrix = np.array(right_cam["camera_matrix"], dtype=np.float64)
            
            # 构建旋转矩阵和平移向量
            R = np.array(stereo["rotation_matrix"], dtype=np.float64)
            T = np.array(stereo["translation_vector"], dtype=np.float64).flatten()
            
            # 构建投影矩阵
            # 左相机投影矩阵: P1 = K1 * [I | 0]
            P1 = left_matrix @ np.hstack([np.eye(3), np.zeros((3, 1))])
            
            # 右相机投影矩阵: P2 = K2 * [R | t]
            P2 = right_matrix @ np.hstack([R, T.reshape(3, 1)])
            
            # 准备图像点坐标
            left_points = np.array([[left_point[0], left_point[1]]], dtype=np.float64)
            right_points = np.array([[right_point[0], right_point[1]]], dtype=np.float64)
            
            # 使用OpenCV的triangulatePoints进行前方交会
            points_4d = cv2.triangulatePoints(P1, P2, left_points.T, right_points.T)
            
            # 转换为3D坐标（齐次坐标转欧几里得坐标）
            point_3d = points_4d[:3, 0] / points_4d[3, 0]
            
            # 打印调试信息
            print(f"OpenCV算法 - 左图像点: ({left_point[0]:.2f}, {left_point[1]:.2f})")
            print(f"OpenCV算法 - 右图像点: ({right_point[0]:.2f}, {right_point[1]:.2f})")
            print(f"OpenCV算法 - 三维坐标: ({point_3d[0]:.2f}, {point_3d[1]:.2f}, {point_3d[2]:.2f})")
            print(f"OpenCV算法 - 坐标系: 以左眼相机为原点，X右Y下Z前")
            
            return point_3d
            
        except Exception as e:
            print(f"OpenCV前方交会算法计算失败: {e}")
            return None
    
    def calculate_3d_coordinates_simple(self, left_point, right_point, camera_params):
        """使用简单的双目立体视觉公式计算三维坐标（以左眼相机为原点）"""
        try:
            # 提取相机参数
            left_cam = camera_params["left_camera"]
            stereo = camera_params["stereo_camera"]
            
            # 获取基线距离（左右相机之间的距离）
            baseline = stereo["translation"]["x"]  # 通常是x方向的平移
            
            # 获取相机的内参
            fx = cam["fx"]  # 像素焦距
            fy = cam["fy"]  # 像素焦距
            cx = cam["cx"]  # 主点x坐标
            cy = cam["cy"]  # 主点y坐标
            
            # 计算视差（disparity）
            # 在标准双目视觉中，左图像中的点在右图像中应该向左偏移
            disparity = left_point[0] - right_point[0]
            
            # 使用标准双目公式计算深度
            if abs(disparity) > 0.1:  # 避免除零
                # 计算深度 Z = (fx * baseline) / disparity
                Z = (fx * baseline) / disparity
                
                # 计算X和Y坐标（在左相机坐标系中）
                X = (left_point[0] - cx) * Z / fx
                Y = (left_point[1] - cy) * Z / fy
                
                point_3d = np.array([X, Y, Z])
                
                # 打印调试信息
                print(f"简单算法 - 左图像点: ({left_point[0]:.2f}, {left_point[1]:.2f})")
                print(f"简单算法 - 右图像点: ({right_point[0]:.2f}, {right_point[1]:.2f})")
                print(f"简单算法 - 视差: {disparity:.2f} 像素")
                print(f"简单算法 - 基线距离: {baseline:.2f} mm")
                print(f"简单算法 - 三维坐标: ({X:.2f}, {Y:.2f}, {Z:.2f})")
                print(f"简单算法 - 坐标系: 以左眼相机为原点，X右Y下Z前")
                
                return point_3d
            else:
                print("视差太小，无法计算三维坐标")
                return None
                
        except Exception as e:
            print(f"简单三维坐标计算失败: {e}")
            return None
    
    def analyze_algorithm_differences(self, left_point, right_point, point_3d_simple, point_3d_opencv):
        """分析两种算法的差异原因"""
        try:
            print(f"\n=== 算法差异分析 ===")
            
            # 计算各坐标分量的差异
            x_diff = abs(point_3d_simple[0] - point_3d_opencv[0])
            y_diff = abs(point_3d_simple[1] - point_3d_opencv[1])
            z_diff = abs(point_3d_simple[2] - point_3d_opencv[2])
            
            print(f"X坐标差异: {x_diff:.1f}mm")
            print(f"Y坐标差异: {y_diff:.1f}mm")
            print(f"Z坐标差异: {z_diff:.1f}mm")
            
            # 分析可能的原因
            print(f"\n=== 差异原因分析 ===")
            
            # 1. 算法原理差异
            print("1. 算法原理差异:")
            print("   - 简单算法: 使用标准双目公式 Z = (fx * baseline) / disparity")
            print("   - OpenCV算法: 使用前方交会三角测量，考虑完整的投影矩阵")
            
            # 2. 坐标系处理差异
            print("2. 坐标系处理差异:")
            print("   - 简单算法: 直接使用左相机坐标系")
            print("   - OpenCV算法: 通过投影矩阵变换，可能涉及坐标系转换")
            
            # 3. 参数使用差异
            print("3. 参数使用差异:")
            print("   - 简单算法: 使用统一相机内参和基线距离")
            print("   - OpenCV算法: 使用统一相机内参、旋转矩阵、平移向量")
            
            # 4. 精度差异
            print("4. 精度差异:")
            print("   - 简单算法: 假设理想的双目几何关系")
            print("   - OpenCV算法: 考虑实际的相机几何关系，包括旋转和平移")
            
            # 5. 数值计算差异
            print("5. 数值计算差异:")
            print("   - 简单算法: 直接代数计算")
            print("   - OpenCV算法: 使用SVD分解等数值方法")
            
            # 判断哪种算法更可靠
            print(f"\n=== 算法可靠性评估 ===")
            
            # 检查视差
            disparity = left_point[0] - right_point[0]
            print(f"视差: {disparity:.2f} 像素")
            
            if abs(disparity) < 5:
                print("警告: 视差太小，可能导致计算不稳定")
            elif abs(disparity) > 200:
                print("警告: 视差太大，可能超出有效测量范围")
            else:
                print("视差在合理范围内")
            
            # 检查深度值
            z_simple = point_3d_simple[2]
            z_opencv = point_3d_opencv[2]
            
            print(f"简单算法深度: {z_simple:.1f}mm")
            print(f"OpenCV算法深度: {z_opencv:.1f}mm")
            
            if z_simple < 0 or z_opencv < 0:
                print("警告: 深度为负值，可能表示计算错误")
            elif z_simple > 10000 or z_opencv > 10000:
                print("警告: 深度值过大，可能超出合理范围")
            else:
                print("深度值在合理范围内")
            
            # 推荐算法
            print(f"\n=== 算法推荐 ===")
            if abs(z_diff) < 50:  # 如果深度差异小于50mm
                print("两种算法结果接近，都可信")
                if abs(disparity) < 20:
                    print("推荐使用OpenCV算法（视差较小，需要更高精度）")
                else:
                    print("推荐使用简单算法（视差较大，计算稳定）")
            else:
                print("两种算法结果差异较大，需要进一步验证")
                if z_simple > 0 and z_opencv < 0:
                    print("推荐使用简单算法（OpenCV算法深度为负）")
                elif z_opencv > 0 and z_simple < 0:
                    print("推荐使用OpenCV算法（简单算法深度为负）")
                else:
                    print("建议检查相机标定参数和图像质量")
            
        except Exception as e:
            print(f"算法差异分析失败: {e}")
    
    def draw_image_with_3d_coordinates(self, image, contours, centroids, point_3d, camera_name):
        """在图像上绘制光斑和三维坐标"""
        try:
            result_image = image.copy()
            
            # 绘制所有检测到的光斑
            for i, (contour, centroid) in enumerate(zip(contours, centroids)):
                cx, cy = centroid
                
                # 计算面积
                area = cv2.contourArea(contour)
                
                # 绘制光斑轮廓（红色圆圈）
                cv2.circle(result_image, (int(cx), int(cy)), 10, (0, 0, 255), 2)
                
                # 绘制十字线（绿色）
                cv2.line(result_image, (int(cx-15), int(cy)), (int(cx+15), int(cy)), (0, 255, 0), 2)
                cv2.line(result_image, (int(cx), int(cy-15)), (int(cx), int(cy+15)), (0, 255, 0), 2)
                
                # 显示质心坐标（绿色，增大字体，位置调整避免重叠）
                coord_text = f"({cx:.1f}, {cy:.1f})"
                cv2.putText(result_image, coord_text, (int(cx) + 25, int(cy) - 20), 
                           cv2.FONT_HERSHEY_SIMPLEX, 1.2, (0, 255, 0), 3)
                
                # 显示面积（黄色，增大字体，位置调整避免重叠）
                area_text = f"A:{area:.0f}"
                cv2.putText(result_image, area_text, (int(cx) - 20, int(cy) + 35), 
                           cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 255, 255), 3)
                
                # 在光斑旁边显示三维坐标（白色）
                if point_3d is not None:
                    coord_3d_text = f"X:{point_3d[0]:.1f} Y:{point_3d[1]:.1f} Z:{point_3d[2]:.1f}"
                    
                    # 在光斑右侧显示三维坐标
                    text_x = int(cx) + 20
                    text_y = int(cy) + 20
                    
                    # 绘制三维坐标文本
                    cv2.putText(result_image, coord_3d_text, (text_x, text_y), 
                               cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1)
            
            return result_image
            
        except Exception as e:
            print(f"Draw {camera_name} camera image failed: {e}")
            return image
    
    def draw_image_with_3d_coordinates_simple(self, image, centroids, point_3d, camera_name):
        """在图像上绘制光斑和三维坐标（简化版本，解决闪烁问题）"""
        try:
            result_image = image.copy()
            
            # 绘制所有检测到的光斑
            for i, centroid in enumerate(centroids):
                cx, cy = centroid
                
                # 绘制光斑轮廓（红色圆圈）
                cv2.circle(result_image, (int(cx), int(cy)), 10, (0, 0, 255), 2)
                
                # 绘制十字线（绿色）
                cv2.line(result_image, (int(cx-15), int(cy)), (int(cx+15), int(cy)), (0, 255, 0), 2)
                cv2.line(result_image, (int(cx), int(cy-15)), (int(cx), int(cy+15)), (0, 255, 0), 2)
                
                # 显示质心坐标（绿色）
                coord_text = f"({cx:.1f}, {cy:.1f})"
                cv2.putText(result_image, coord_text, (int(cx) + 15, int(cy) - 15), 
                           cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0, 255, 0), 1)
                
                # 在光斑旁边显示三维坐标（白色）
                if point_3d is not None:
                    coord_3d_text = f"X:{point_3d[0]:.1f} Y:{point_3d[1]:.1f} Z:{point_3d[2]:.1f}"
                    
                    # 在光斑右侧显示三维坐标
                    text_x = int(cx) + 20
                    text_y = int(cy) + 20
                    
                    # 绘制三维坐标文本
                    cv2.putText(result_image, coord_3d_text, (text_x, text_y), 
                               cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1)
            
            return result_image
            
        except Exception as e:
            print(f"Draw {camera_name} camera image failed: {e}")
            return image
    
    def stop(self):
        """停止线程"""
        self.is_running = False


class ZEDCameraThread(QThread):
    """ZED相机图像捕获线程"""
    
    # 信号定义
    left_image_ready = pyqtSignal(object)  # 改为object类型以支持tuple
    right_image_ready = pyqtSignal(object)  # 改为object类型以支持tuple
    exposure_changed = pyqtSignal(int)
    error_occurred = pyqtSignal(str)
    blob_detection_ready = pyqtSignal(list, list, list, list)  # 发送光斑检测结果
    
    def __init__(self, zed_camera, main_window=None):
        super().__init__()
        self.zed = zed_camera
        self.main_window = main_window  # 保存主窗口引用
        self.is_running = False
        self.current_exposure = 0
        self.min_exposure = 0
        self.max_exposure = 0
        
        # 聚焦控制
        self.auto_focus_enabled = True  # 默认启用自动聚焦
        
        # 初始化光斑检测器
        self.blob_detector = BlobDetector()
        self.blob_enabled = True  # 默认启用光斑检测
        
        # 获取曝光范围
        try:
            exposure_info = self.zed.get_camera_settings(sl.VIDEO_SETTINGS.EXPOSURE)
            if len(exposure_info) >= 3:
                self.current_exposure = exposure_info[0]
                self.min_exposure = exposure_info[1]
                self.max_exposure = exposure_info[2]
            else:
                # 如果获取失败，使用默认值
                self.min_exposure = 0
                self.max_exposure = 100
                self.current_exposure = 50
        except Exception as e:
            # 使用默认值
            self.min_exposure = 0
            self.max_exposure = 100
            self.current_exposure = 50
    
    def set_exposure(self, exposure_value):
        """设置曝光值"""
        try:
            # 确保曝光值在有效范围内
            exposure_value = max(self.min_exposure, min(self.max_exposure, exposure_value))
            
            # 设置曝光
            result = self.zed.set_camera_settings(sl.VIDEO_SETTINGS.EXPOSURE, exposure_value)
            
            # 检查设置是否成功
            if result == sl.ERROR_CODE.SUCCESS:
                self.current_exposure = exposure_value
                # 发送信号通知曝光变化
                self.exposure_changed.emit(exposure_value)
                print(f"曝光已设置为: {exposure_value}")
                return True
            else:
                print(f"设置曝光失败，错误代码: {result}")
                return False
        except Exception as e:
            print(f"设置曝光异常: {e}")
            return False
    
    def set_auto_focus(self, enabled):
        """设置自动聚焦"""
        try:
            if enabled:
                # 启用自动聚焦
                self.zed.set_camera_settings(sl.VIDEO_SETTINGS.AEC_AGC, True)
                self.zed.set_camera_settings(sl.VIDEO_SETTINGS.AEC_AGC_ROI, sl.Rect(0, 0, 0, 0))  # 全区域自动聚焦
            else:
                # 禁用自动聚焦
                self.zed.set_camera_settings(sl.VIDEO_SETTINGS.AEC_AGC, False)
            
            self.auto_focus_enabled = enabled
            return True
        except Exception as e:
            return False
    
    def get_current_exposure(self):
        """获取当前曝光值"""
        try:
            # 使用更安全的方法获取曝光值
            exposure_info = self.zed.get_camera_settings(sl.VIDEO_SETTINGS.EXPOSURE)
            
            # 检查返回值类型
            if isinstance(exposure_info, tuple) and len(exposure_info) >= 1:
                exposure_value = exposure_info[0]
                if isinstance(exposure_value, (int, float)):
                    return int(exposure_value)
                elif hasattr(exposure_value, 'value'):  # 如果是枚举类型
                    return int(exposure_value.value)
                else:
                    return self.current_exposure
            elif isinstance(exposure_info, (int, float)):
                return int(exposure_info)
            else:
                return self.current_exposure
                
        except Exception as e:
            print(f"获取曝光值失败: {e}")
            return self.current_exposure
    
    def trigger_manual_focus(self):
        """触发手动聚焦"""
        try:
            # 保存当前曝光设置
            current_exposure = self.get_current_exposure()
            print(f"手动聚焦前保存曝光值: {current_exposure}")
            
            # 使用更安全的聚焦方法，只调整聚焦，不改变曝光
            try:
                # 简化的聚焦方法：只重新触发自动聚焦，不改变其他设置
                if self.auto_focus_enabled:
                    # 如果当前是自动聚焦，先禁用再启用来重新聚焦
                    self.zed.set_camera_settings(sl.VIDEO_SETTINGS.AEC_AGC, False)
                    time.sleep(0.1)
                    self.zed.set_camera_settings(sl.VIDEO_SETTINGS.AEC_AGC, True)
                    time.sleep(0.5)  # 等待聚焦完成
                else:
                    # 如果当前是手动聚焦，临时启用自动聚焦
                    self.zed.set_camera_settings(sl.VIDEO_SETTINGS.AEC_AGC, True)
                    time.sleep(0.5)  # 等待聚焦完成
                    self.zed.set_camera_settings(sl.VIDEO_SETTINGS.AEC_AGC, False)
                
                # 强制恢复曝光设置
                print(f"聚焦完成后恢复曝光值: {current_exposure}")
                if isinstance(current_exposure, (int, float)):
                    success = self.set_exposure(int(current_exposure))
                    if success:
                        print(f"✓ 曝光值恢复成功: {current_exposure}")
                    else:
                        print(f"⚠ 曝光值恢复失败: {current_exposure}")
                
                return True
                
            except Exception as focus_error:
                print(f"聚焦过程出错: {focus_error}")
                # 确保曝光设置不变
                if isinstance(current_exposure, (int, float)):
                    self.set_exposure(int(current_exposure))
                return True
                
        except Exception as e:
            print(f"手动聚焦失败: {e}")
            return False
    
    def run(self):
        """线程运行函数"""
        self.is_running = True

        # 创建图像矩阵
        image_left = sl.Mat()
        image_right = sl.Mat()
        runtime_parameters = sl.RuntimeParameters()

        while self.is_running:
            try:
                if self.zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS:
                    # 获取左右眼图像
                    self.zed.retrieve_image(image_left, sl.VIEW.LEFT)
                    self.zed.retrieve_image(image_right, sl.VIEW.RIGHT)
                    
                    # 转换为OpenCV格式
                    left_cv_image = np.array(image_left.get_data())
                    right_cv_image = np.array(image_right.get_data())
                    
                    # 确保图像是3通道RGB格式
                    if left_cv_image.shape[-1] == 4:
                        left_cv_image = cv2.cvtColor(left_cv_image, cv2.COLOR_RGBA2RGB)
                    if right_cv_image.shape[-1] == 4:
                        right_cv_image = cv2.cvtColor(right_cv_image, cv2.COLOR_RGBA2RGB)
                    
                    # 转换为BGR格式（OpenCV默认格式）
                    left_bgr_original = cv2.cvtColor(left_cv_image, cv2.COLOR_RGB2BGR)
                    right_bgr_original = cv2.cvtColor(right_cv_image, cv2.COLOR_RGB2BGR)
                    
                    # 获取原始图像尺寸
                    left_height, left_width = left_bgr_original.shape[:2]
                    right_height, right_width = right_bgr_original.shape[:2]
                    
                    # 光斑检测（如果启用）- 使用原始分辨率进行检测
                    if hasattr(self, 'blob_enabled') and self.blob_enabled:
                        left_contours, _ = self.blob_detector.detect_blobs(left_bgr_original)
                        right_contours, _ = self.blob_detector.detect_blobs(right_bgr_original)
                        
                        # 计算质心
                        left_centroids = []
                        right_centroids = []
                        
                        for contour in left_contours:
                            M = cv2.moments(contour)
                            if M["m00"] != 0:
                                cx = M["m10"] / M["m00"]
                                cy = M["m01"] / M["m00"]
                                left_centroids.append((cx, cy))
                        
                        for contour in right_contours:
                            M = cv2.moments(contour)
                            if M["m00"] != 0:
                                cx = M["m10"] / M["m00"]
                                cy = M["m01"] / M["m00"]
                                right_centroids.append((cx, cy))
                        
                        # 发送光斑检测结果到三维坐标计算线程
                        self.blob_detection_ready.emit(left_centroids, right_centroids, left_contours, right_contours)
                        
                        # 添加调试信息
                        if len(left_centroids) > 0 or len(right_centroids) > 0:
                            print(f"检测到光斑 - 左: {len(left_centroids)} 个, 右: {len(right_centroids)} 个")
                            if len(left_centroids) > 0:
                                print(f"左眼光斑坐标: {left_centroids}")
                            if len(right_centroids) > 0:
                                print(f"右眼光斑坐标: {right_centroids}")
                        
                        # 绘制检测结果 - 使用原始分辨率
                        left_result = self.draw_blobs_simple(left_bgr_original, left_contours, left_centroids, "Left")
                        right_result = self.draw_blobs_simple(right_bgr_original, right_contours, right_centroids, "Right")
                    else:
                        left_result = left_bgr_original
                        right_result = right_bgr_original
                    
                    # 发送图像信号（包含原始图像数据）
                    self.left_image_ready.emit((left_result, left_bgr_original))
                    self.right_image_ready.emit((right_result, right_bgr_original))
                    
                    # 调试信息（仅在开始时打印）
                    if not hasattr(self, '_debug_printed'):
                        print(f"真实图像尺寸 - 左: {left_result.shape}, 右: {right_result.shape}")
                        print(f"图像数据类型 - 左: {left_result.dtype}, 右: {right_result.dtype}")
                        print(f"检测到光斑 - 左: {len(left_centroids)} 个, 右: {len(right_centroids)} 个")
                        self._debug_printed = True
                
                time.sleep(0.033)  # 约30FPS
                
            except Exception as e:
                time.sleep(0.1)
    
    def draw_blobs_simple(self, image, contours, centroids, camera_name):
        """绘制光斑（简化版本，不包含三维坐标）"""
        try:
            result_image = image.copy()
            
            for i, contour in enumerate(contours):
                if i >= len(centroids):
                    continue
                    
                cx, cy = centroids[i]
                
                # 计算面积
                area = cv2.contourArea(contour)
                
                # 计算轮廓的边界框
                x, y, w, h = cv2.boundingRect(contour)
                size = max(w, h)
                
                # 绘制轮廓
                cv2.drawContours(result_image, [contour], -1, (255, 0, 0), 2)
                
                # 绘制十字线（更细）
                color = (0, 255, 0)  # 绿色
                thickness = 1  # 更细的线条
                length = max(8, size // 3)
                
                # 水平线
                cv2.line(result_image, (int(cx - length), int(cy)), (int(cx + length), int(cy)), color, thickness)
                # 垂直线
                cv2.line(result_image, (int(cx), int(cy - length)), (int(cx), int(cy + length)), color, thickness)
                
                # 显示质心坐标（绿色，增大字体，位置调整避免重叠）
                coord_text = f"({cx:.1f}, {cy:.1f})"
                cv2.putText(result_image, coord_text, (int(cx) + 25, int(cy) - 20), 
                           cv2.FONT_HERSHEY_SIMPLEX, 1.2, (0, 255, 0), 3)
                
                # 显示面积（黄色，增大字体，位置调整避免重叠）
                area_text = f"A:{area:.0f}"
                cv2.putText(result_image, area_text, (int(cx) - 20, int(cy) + 35), 
                           cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 255, 255), 3)
            
            return result_image
            
        except Exception as e:
            print(f"Draw {camera_name} camera image failed: {e}")
            return image
    
    def draw_blobs_with_3d_simple(self, image, contours, centroids, three_d_coordinates, camera_name):
        """绘制光斑并显示三维坐标（简化版本）"""
        try:
            result_image = image.copy()
            
            for i, contour in enumerate(contours):
                if i >= len(centroids):
                    continue
                    
                cx, cy = centroids[i]
                
                # 计算轮廓的边界框
                x, y, w, h = cv2.boundingRect(contour)
                size = max(w, h)
                
                # 绘制轮廓
                cv2.drawContours(result_image, [contour], -1, (255, 0, 0), 2)
                
                # 绘制十字线（更细）
                color = (0, 255, 0)  # 绿色
                thickness = 1  # 更细的线条
                length = max(8, size // 3)
                
                # 水平线
                cv2.line(result_image, (int(cx - length), int(cy)), (int(cx + length), int(cy)), color, thickness)
                # 垂直线
                cv2.line(result_image, (int(cx), int(cy - length)), (int(cx), int(cy + length)), color, thickness)
                
                # 绘制质心坐标文本（保留一位小数，增大字体，位置调整避免重叠）
                coord_text = f"({cx:.1f}, {cy:.1f})"
                cv2.putText(result_image, coord_text, (int(cx) + 25, int(cy) - 20), 
                           cv2.FONT_HERSHEY_SIMPLEX, 1.2, color, 3)
                
                # 绘制光斑编号和面积（增大字体，位置调整避免重叠）
                area = cv2.contourArea(contour)
                info_text = f"#{i+1} A:{area:.0f}"
                cv2.putText(result_image, info_text, (int(cx) - 20, int(cy) + 35), 
                           cv2.FONT_HERSHEY_SIMPLEX, 1.0, (255, 255, 0), 3)
                
                # 查找并显示三维坐标
                for idx, point_3d in three_d_coordinates:
                    if idx == i:
                        # 显示三维坐标（白色，稍大字体）
                        x_coord = point_3d[0]
                        y_coord = point_3d[1]
                        z_coord = point_3d[2]
                        
                        # 格式化坐标显示
                        coord_3d_text = f"3D: X{x_coord:.1f} Y{y_coord:.1f} Z{z_coord:.1f}"
                        
                        # 在光斑下方显示三维坐标
                        text_x = max(5, int(cx) - 50)  # 确保文本不超出图像边界
                        text_y = int(cy) + 35
                        
                        # 绘制白色文本，稍大字体
                        cv2.putText(result_image, coord_3d_text, 
                                   (text_x, text_y), 
                                   cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2)
                break
            
            return result_image
            
        except Exception as e:
            print(f"Draw {camera_name} camera image failed: {e}")
            return image
    
    def draw_blobs_with_3d(self, image, contours, centroids, point_3d, is_left_camera):
        """绘制光斑并显示三维坐标"""
        try:
            result_image = image.copy()
            
            for i, contour in enumerate(contours):
                if i >= len(centroids):
                    continue
                    
                cx, cy = centroids[i]
                
                # 计算轮廓的边界框
                x, y, w, h = cv2.boundingRect(contour)
                size = max(w, h)
                
                # 绘制轮廓
                cv2.drawContours(result_image, [contour], -1, (255, 0, 0), 2)
                
                # 绘制十字线（更细）
                color = (0, 255, 0)  # 绿色
                thickness = 1  # 更细的线条
                length = max(8, size // 3)
                
                # 水平线
                cv2.line(result_image, (int(cx - length), int(cy)), (int(cx + length), int(cy)), color, thickness)
                # 垂直线
                cv2.line(result_image, (int(cx), int(cy - length)), (int(cx), int(cy + length)), color, thickness)
                
                # 绘制质心坐标文本（保留一位小数，增大字体，位置调整避免重叠）
                coord_text = f"({cx:.1f}, {cy:.1f})"
                cv2.putText(result_image, coord_text, (int(cx) + 25, int(cy) - 20), 
                           cv2.FONT_HERSHEY_SIMPLEX, 1.2, color, 3)
                
                # 绘制光斑编号和面积（增大字体，位置调整避免重叠）
                area = cv2.contourArea(contour)
                info_text = f"#{i+1} A:{area:.0f}"
                cv2.putText(result_image, info_text, (int(cx) - 20, int(cy) + 35), 
                           cv2.FONT_HERSHEY_SIMPLEX, 1.0, (255, 255, 0), 3)
                

            
            return result_image
            
        except Exception as e:
            return image
    
    def calculate_3d_coordinates(self, left_point, right_point, camera_params):
        """根据左右图像中的点计算三维坐标"""
        try:
            if camera_params is None:
                print("相机参数未提供")
                return None
            
            # 提取相机参数
            left_cam = camera_params["left_camera"]
            right_cam = camera_params["right_camera"]
            stereo = camera_params["stereo_camera"]
            
            # 构建相机矩阵
            left_matrix = np.array([
                [left_cam["fx"], 0, left_cam["cx"]],
                [0, left_cam["fy"], left_cam["cy"]],
                [0, 0, 1]
            ])
            
            right_matrix = np.array([
                [right_cam["fx"], 0, right_cam["cx"]],
                [0, right_cam["fy"], right_cam["cy"]],
                [0, 0, 1]
            ])
            
            # 构建旋转矩阵
            R = np.array([
                [stereo["rotation"]["r00"], stereo["rotation"]["r01"], stereo["rotation"]["r02"]],
                [stereo["rotation"]["r10"], stereo["rotation"]["r11"], stereo["rotation"]["r12"]],
                [stereo["rotation"]["r20"], stereo["rotation"]["r21"], stereo["rotation"]["r22"]]
            ])
            
            # 构建平移向量
            T = np.array([stereo["translation"]["x"], 
                         stereo["translation"]["y"], 
                         stereo["translation"]["z"]])
            
            # 打印相机参数信息
            print(f"左相机矩阵:\n{left_matrix}")
            print(f"右相机矩阵:\n{right_matrix}")
            print(f"旋转矩阵:\n{R}")
            print(f"平移向量: {T}")
            
            # 使用OpenCV的triangulatePoints函数
            # 准备点坐标
            left_points = np.array([[left_point[0], left_point[1]]], dtype=np.float32)
            right_points = np.array([[right_point[0], right_point[1]]], dtype=np.float32)
            
            # 构建投影矩阵
            left_proj = left_matrix @ np.hstack([np.eye(3), np.zeros((3, 1))])
            right_proj = right_matrix @ np.hstack([R, T.reshape(3, 1)])
            
            print(f"左投影矩阵:\n{left_proj}")
            print(f"右投影矩阵:\n{right_proj}")
            
            # 使用OpenCV的triangulatePoints
            points_4d = cv2.triangulatePoints(left_proj, right_proj, left_points.T, right_points.T)
            
            # 转换为3D坐标
            point_3d = points_4d[:3, 0] / points_4d[3, 0]
            
            # 打印调试信息
            print(f"左图像点: ({left_point[0]:.2f}, {left_point[1]:.2f})")
            print(f"右图像点: ({right_point[0]:.2f}, {right_point[1]:.2f})")
            print(f"三维坐标: ({point_3d[0]:.2f}, {point_3d[1]:.2f}, {point_3d[2]:.2f})")
            
            # 坐标系说明
            print("坐标系定义:")
            print("X轴: 指向右方向（从相机中心向右）")
            print("Y轴: 指向下方向（从相机中心向下）")
            print("Z轴: 指向前方（从相机中心向前，即光轴方向）")
            print("Z轴为负值表示物体在相机前方，这是正常的")
            
            return point_3d
            
        except Exception as e:
            print(f"计算三维坐标失败: {e}")
            return None
    
    def calculate_3d_coordinates_simple(self, left_point, right_point, camera_params):
        """使用简单的双目立体视觉公式计算三维坐标（以左眼相机为原点）"""
        try:
            if camera_params is None:
                print("相机参数未提供")
                return None
            
            # 提取相机参数
            left_cam = camera_params["left_camera"]
            stereo = camera_params["stereo_camera"]
            
            # 获取基线距离（左右相机之间的距离）
            baseline = abs(stereo["translation_vector"][0][0])  # X方向平移的绝对值
            
            # 获取左相机内参
            fx = left_cam["camera_matrix"][0][0]  # 焦距
            fy = left_cam["camera_matrix"][1][1]  # 焦距
            cx = left_cam["camera_matrix"][0][2]  # 主点x坐标
            cy = left_cam["camera_matrix"][1][2]  # 主点y坐标
            
            # 计算视差
            disparity = left_point[0] - right_point[0]
            
            if abs(disparity) < 0.1:
                print("视差太小，无法计算")
                return None
            
            # 使用双目公式计算深度
            Z = (fx * baseline) / disparity
            
            # 计算X和Y坐标
            X = (left_point[0] - cx) * Z / fx
            Y = (left_point[1] - cy) * Z / fy
            
            point_3d = np.array([X, Y, Z])
            
            # 打印调试信息
            print(f"简单算法 - 左图像点: ({left_point[0]:.2f}, {left_point[1]:.2f})")
            print(f"简单算法 - 右图像点: ({right_point[0]:.2f}, {right_point[1]:.2f})")
            print(f"简单算法 - 视差: {disparity:.2f} 像素")
            print(f"简单算法 - 基线距离: {baseline:.2f} mm")
            print(f"简单算法 - 三维坐标: ({X:.2f}, {Y:.2f}, {Z:.2f})")
            print(f"简单算法 - 坐标系: 以左眼相机为原点，X右Y下Z前")
            
            return point_3d
            
        except Exception as e:
            print(f"简单三维坐标计算失败: {e}")
            return None
    
    def calculate_3d_from_left_camera(self, image_point, camera_params):
        """从左相机图像点计算三维坐标（假设固定深度）"""
        try:
            if camera_params is None:
                print("相机参数未提供")
                return None
            
            # 提取左相机参数
            left_cam = camera_params["left_camera"]
            
            # 构建相机内参矩阵
            fx = left_cam["camera_matrix"][0][0]
            fy = left_cam["camera_matrix"][1][1]
            cx = left_cam["camera_matrix"][0][2]
            cy = left_cam["camera_matrix"][1][2]
            
            # 假设光斑在固定深度（比如1000mm）
            Z = 1000.0  # 固定深度，单位mm
            
            # 使用针孔相机模型计算X和Y
            X = (image_point[0] - cx) * Z / fx
            Y = (image_point[1] - cy) * Z / fy
            
            return np.array([X, Y, Z])
            
        except Exception as e:
            print(f"左相机三维坐标计算失败: {e}")
            return None
    
    def calculate_3d_from_right_camera(self, image_point, camera_params):
        """从右相机图像点计算三维坐标（转换到左相机坐标系）"""
        try:
            if camera_params is None:
                print("相机参数未提供")
                return None
            
            # 提取相机参数
            left_cam = camera_params["left_camera"]
            right_cam = camera_params["right_camera"]
            stereo = camera_params["stereo_camera"]
            
            # 构建右相机内参矩阵
            fx = right_cam["camera_matrix"][0][0]
            fy = right_cam["camera_matrix"][1][1]
            cx = right_cam["camera_matrix"][0][2]
            cy = right_cam["camera_matrix"][1][2]
            
            # 假设光斑在固定深度（比如1000mm）
            Z = 1000.0  # 固定深度，单位mm
            
            # 在右相机坐标系下计算X和Y
            X_right = (image_point[0] - cx) * Z / fx
            Y_right = (image_point[1] - cy) * Z / fy
            
            # 构建旋转矩阵（从欧拉角）
            rx, ry, rz = stereo["rotation"]["rx"], stereo["rotation"]["ry"], stereo["rotation"]["rz"]
            
            Rx = np.array([[1, 0, 0],
                          [0, np.cos(rx), -np.sin(rx)],
                          [0, np.sin(rx), np.cos(rx)]])
            
            Ry = np.array([[np.cos(ry), 0, np.sin(ry)],
                          [0, 1, 0],
                          [-np.sin(ry), 0, np.cos(ry)]])
            
            Rz = np.array([[np.cos(rz), -np.sin(rz), 0],
                          [np.sin(rz), np.cos(rz), 0],
                          [0, 0, 1]])
            
            R = Rz @ Ry @ Rx
            
            # 平移向量
            T = np.array([stereo["translation"]["x"], 
                         stereo["translation"]["y"], 
                         stereo["translation"]["z"]])
            
            # 将右相机坐标系下的点转换到左相机坐标系
            point_right = np.array([X_right, Y_right, Z])
            point_left = R @ point_right + T
            
            return point_left
            
        except Exception as e:
            print(f"右相机三维坐标计算失败: {e}")
            return None
    
    def calculate_3d_coordinates_advanced(self, left_point, right_point, camera_params):
        """使用OpenCV高级立体视觉算法计算三维坐标"""
        try:
            if camera_params is None:
                print("相机参数未提供")
                return None
            
            # 提取相机参数
            left_cam = camera_params["left_camera"]
            right_cam = camera_params["right_camera"]
            stereo = camera_params["stereo_camera"]
            
            # 构建相机矩阵
            left_matrix = np.array([
                [left_cam["fx"], 0, left_cam["cx"]],
                [0, left_cam["fy"], left_cam["cy"]],
                [0, 0, 1]
            ], dtype=np.float32)
            
            right_matrix = np.array([
                [right_cam["fx"], 0, right_cam["cx"]],
                [0, right_cam["fy"], right_cam["cy"]],
                [0, 0, 1]
            ], dtype=np.float32)
            
            # 构建旋转矩阵
            R = np.array([
                [stereo["rotation"]["r00"], stereo["rotation"]["r01"], stereo["rotation"]["r02"]],
                [stereo["rotation"]["r10"], stereo["rotation"]["r11"], stereo["rotation"]["r12"]],
                [stereo["rotation"]["r20"], stereo["rotation"]["r21"], stereo["rotation"]["r22"]]
            ], dtype=np.float32)
            
            # 构建平移向量
            T = np.array([stereo["translation"]["x"], 
                         stereo["translation"]["y"], 
                         stereo["translation"]["z"]], dtype=np.float32)
            
            # 计算空间直线交会
            point_3d = self.intersect_rays_least_squares(left_point, right_point, 
                                                       left_matrix, right_matrix, R, T)
            
            print(f"空间直线交会算法结果:")
            print(f"左图像点: ({left_point[0]:.2f}, {left_point[1]:.2f})")
            print(f"右图像点: ({right_point[0]:.2f}, {right_point[1]:.2f})")
            print(f"三维坐标: ({point_3d[0]:.2f}, {point_3d[1]:.2f}, {point_3d[2]:.2f})")
            print(f"坐标系: X右 Y下 Z前（正值）")
            
            return point_3d
            
        except Exception as e:
            print(f"高级三维坐标计算失败: {e}")
            return None
    

    

    

    
    def stop(self):
        """停止线程"""
        self.is_running = False


class FullSizeImageDialog(QDialog):
    """原尺寸图像显示对话框"""
    
    def __init__(self, image, title="原尺寸图像", parent=None, camera_thread=None):
        super().__init__(parent)
        self.setWindowTitle(title)
        self.setModal(False)  # 非模态对话框
        self.camera_thread = camera_thread
        self.is_left_camera = "左眼" in title
        self.main_window = None
        
        # 查找主窗口
        parent_widget = parent
        while parent_widget:
            if hasattr(parent_widget, 'camera_thread'):
                self.main_window = parent_widget
            break
            parent_widget = parent_widget.parent()
        
        # 设置窗口大小和位置
        screen = QApplication.primaryScreen().geometry()
        window_width = int(min(screen.width() * 0.8, image.shape[1]))
        window_height = int(min(screen.height() * 0.8, image.shape[0]))
        self.resize(window_width, window_height)
        
        # 创建布局
        layout = QVBoxLayout(self)
        
        # 创建滚动区域
        scroll_area = QScrollArea()
        scroll_area.setWidgetResizable(True)
        layout.addWidget(scroll_area)
        
        # 创建图像标签
        self.image_label = QLabel()
        
        # 设置标签大小策略
        self.image_label.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding)
        self.image_label.setAlignment(Qt.AlignCenter)
        self.image_label.setMinimumSize(100, 100)  # 设置最小尺寸
        
        # 转换图像格式并显示
        self.update_image_display(image)
        
        # 将标签添加到滚动区域
        scroll_area.setWidget(self.image_label)
        
        # 添加分辨率信息标签
        height, width, channel = image.shape
        self.resolution_label = QLabel(f"相机真实分辨率: {width} × {height} 像素")
        self.resolution_label.setAlignment(Qt.AlignCenter)
        self.resolution_label.setStyleSheet("font-size: 14px; font-weight: bold; color: #333; padding: 5px; background-color: #f0f0f0; border: 1px solid #ccc;")
        layout.addWidget(self.resolution_label)
        
        # 添加按钮布局
        button_layout = QHBoxLayout()
        
        # 实时刷新开关
        self.refresh_checkbox = QCheckBox("实时刷新")
        self.refresh_checkbox.setChecked(True)
        self.refresh_checkbox.toggled.connect(self.toggle_refresh)
        button_layout.addWidget(self.refresh_checkbox)
        
        # 最大化按钮
        maximize_btn = QPushButton("最大化")
        maximize_btn.setMaximumWidth(80)
        maximize_btn.clicked.connect(self.toggle_maximize)
        button_layout.addWidget(maximize_btn)
        
        # 关闭按钮
        close_btn = QPushButton("关闭")
        close_btn.setMaximumWidth(80)
        close_btn.clicked.connect(self.close)
        button_layout.addWidget(close_btn)
        
        layout.addLayout(button_layout)
        
        # 保存原始窗口大小
        self.original_size = None
        self.is_maximized = False
        
        # 设置定时器用于实时刷新
        self.refresh_timer = QTimer()
        self.refresh_timer.timeout.connect(self.update_image)
        self.refresh_timer.start(100)  # 每100ms刷新一次
    
    def toggle_maximize(self):
        """切换最大化状态"""
        if hasattr(self, 'is_maximized') and self.is_maximized:
            # 恢复原始大小
            if hasattr(self, 'original_size') and self.original_size:
                self.resize(self.original_size)
            self.is_maximized = False
        else:
            # 保存当前大小并最大化
            self.original_size = self.size()
            self.showMaximized()
            self.is_maximized = True
            
        # 延迟更新图像显示，确保窗口状态已经调整完成
        QTimer.singleShot(200, self.force_update_image)
    
    def toggle_refresh(self, enabled):
        """切换实时刷新"""
        if enabled:
            self.refresh_timer.start(100)
        else:
            self.refresh_timer.stop()
    
    def update_image_display(self, image):
        """更新图像显示"""
        try:
            # 确保图像是BGR格式
            if len(image.shape) == 3 and image.shape[2] == 3:
                # 转换为RGB格式用于QImage显示
                rgb_image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
                height, width, channel = rgb_image.shape
                bytes_per_line = 3 * width
                q_image = QImage(rgb_image.data, width, height, bytes_per_line, QImage.Format_RGB888)
                pixmap = QPixmap.fromImage(q_image)
                
                # 根据窗口状态调整图像大小
                if hasattr(self, 'is_maximized') and self.is_maximized:
                    # 最大化时，让图像适应窗口大小，保持宽高比
                    window_size = self.size()
                    scaled_pixmap = pixmap.scaled(
                        window_size.width() - 50,  # 留一些边距
                        window_size.height() - 100,  # 留一些边距给按钮和标签
                        Qt.KeepAspectRatio,
                        Qt.SmoothTransformation
                    )
                    self.image_label.setPixmap(scaled_pixmap)
                else:
                    # 非最大化时，显示原尺寸
                    self.image_label.setPixmap(pixmap)
        except Exception as e:
            print(f"更新图像显示失败: {e}")
    
    def update_image(self):
        """更新图像显示"""
        if not self.refresh_checkbox.isChecked():
            return
        
        try:
            if self.main_window:
                if self.is_left_camera and self.main_window.current_left_image is not None:
                    # 更新左眼图像
                    self.update_image_display(self.main_window.current_left_image)
                elif not self.is_left_camera and self.main_window.current_right_image is not None:
                    # 更新右眼图像
                    self.update_image_display(self.main_window.current_right_image)
        except Exception as e:
            print(f"更新图像失败: {e}")
    
    def force_update_image(self):
        """强制更新图像显示（用于窗口状态变化时）"""
        try:
            if self.main_window:
                if self.is_left_camera and self.main_window.current_left_image is not None:
                    self.update_image_display(self.main_window.current_left_image)
                elif not self.is_left_camera and self.main_window.current_right_image is not None:
                    self.update_image_display(self.main_window.current_right_image)
        except Exception as e:
            print(f"强制更新图像失败: {e}")
    
    def resizeEvent(self, event):
        """窗口大小变化事件"""
        super().resizeEvent(event)
        # 延迟更新图像显示，确保窗口大小已经调整完成
        QTimer.singleShot(100, self.force_update_image)
    
    def closeEvent(self, event):
        """关闭事件"""
        self.refresh_timer.stop()
        event.accept()


class ClickableImageLabel(QLabel):
    """可点击的图像标签"""
    
    def __init__(self, text="", parent=None):
        super().__init__(text, parent)
        self.setCursor(Qt.PointingHandCursor)  # 鼠标悬停时显示手型光标
        self.original_image = None
        self.image_title = ""
    
    def setOriginalImage(self, image, title=""):
        """设置原始图像数据"""
        self.original_image = image
        self.image_title = title
    
    def mouseDoubleClickEvent(self, event: QMouseEvent):
        """双击事件处理"""
        if self.original_image is not None:
            # 从父窗口获取相机线程
            camera_thread = None
            parent = self.parent()
            while parent:
                if hasattr(parent, 'camera_thread'):
                    camera_thread = parent.camera_thread
                    break
                parent = parent.parent()
            
            dialog = FullSizeImageDialog(self.original_image, self.image_title, self.parent(), camera_thread)
            dialog.show()


class ZEDExposureControlGUI(QMainWindow):
    """ZED相机曝光控制GUI界面"""
    
    def __init__(self):
        super().__init__()
        self.zed = None
        self.camera_thread = None
        self.coordinate_thread = None  # 三维坐标计算线程
        
        # 获取当前程序文件所在目录
        current_dir = os.path.dirname(os.path.abspath(__file__))
        self.img_folder = os.path.join(current_dir, "img")  # 图片保存文件夹
        self.config_file = os.path.join(current_dir, "zed_camera_config.json")  # 配置文件
        
        self.img_counter = 1     # 图片编号计数器
        self.current_left_image = None   # 当前左眼图像
        self.current_right_image = None  # 当前右眼图像
        self.full_size_dialogs = []  # 保存打开的全尺寸对话框
        
        # 光斑检测结果
        self.left_centroids = []
        self.right_centroids = []
        self.left_contours = []
        self.right_contours = []
        
        self.init_ui()
        self.load_config()  # 加载配置
        self.init_camera()
        
        # 创建定时器来同步曝光值
        self.exposure_sync_timer = QTimer()
        self.exposure_sync_timer.timeout.connect(self.sync_exposure_value)
        self.exposure_sync_timer.start(2000)  # 每2秒同步一次
    
    def init_ui(self):
        """初始化用户界面"""
        self.setWindowTitle("ZED相机曝光控制")
        self.setGeometry(100, 100, 1400, 900)
        
        # 创建中央部件
        central_widget = QWidget()
        self.setCentralWidget(central_widget)
        
        # 创建主布局 - 垂直布局
        main_layout = QVBoxLayout(central_widget)
        
        # 上半部分：图像显示区域和控制面板
        top_layout = QHBoxLayout()
        
        # 图像显示区域 - 左右排布
        image_layout = QHBoxLayout()
        
        # 左眼图像
        left_group = QGroupBox("左眼相机")
        left_layout = QVBoxLayout(left_group)
        self.left_image_label = ClickableImageLabel("等待图像...")
        self.left_image_label.setMinimumSize(640, 480)
        self.left_image_label.setAlignment(Qt.AlignCenter)
        self.left_image_label.setStyleSheet("border: 2px solid gray;")
        left_layout.addWidget(self.left_image_label)
        image_layout.addWidget(left_group)
        
        # 右眼图像
        right_group = QGroupBox("右眼相机")
        right_layout = QVBoxLayout(right_group)
        self.right_image_label = ClickableImageLabel("等待图像...")
        self.right_image_label.setMinimumSize(640, 480)
        self.right_image_label.setAlignment(Qt.AlignCenter)
        self.right_image_label.setStyleSheet("border: 2px solid gray;")
        right_layout.addWidget(self.right_image_label)
        image_layout.addWidget(right_group)
        
        top_layout.addLayout(image_layout)
        
        # 将上半部分添加到主布局
        main_layout.addLayout(top_layout)
        
        # 下半部分：三维坐标显示区域
        bottom_layout = QHBoxLayout()
        
        # 三维坐标显示区域（只显示OpenCV算法结果）
        coord_group = QGroupBox("三维坐标测量结果")
        coord_layout = QHBoxLayout(coord_group)
        
        # OpenCV算法结果 - 横向显示
        self.opencv_coord_label = QLabel("等待计算...")
        self.opencv_coord_label.setAlignment(Qt.AlignCenter)
        self.opencv_coord_label.setStyleSheet("""
            QLabel {
                font-size: 36px;
                font-weight: bold;
                color: #4CAF50;
                background-color: #E8F5E8;
                border: 3px solid #4CAF50;
                border-radius: 8px;
                padding: 30px;
                min-height: 120px;
            }
        """)
        coord_layout.addWidget(self.opencv_coord_label)
        
        # 将坐标显示区域添加到下半部分
        bottom_layout.addWidget(coord_group)
        
        # 右侧控制面板
        control_widget = QWidget()
        control_widget.setMaximumWidth(300)  # 限制控制面板宽度
        control_layout = QVBoxLayout(control_widget)
        
        # 相机设置组
        camera_group = QGroupBox("相机设置")
        camera_layout = QVBoxLayout(camera_group)
        
        # 分辨率设置
        resolution_layout = QHBoxLayout()
        resolution_layout.addWidget(QLabel("分辨率:"))
        self.resolution_combo = QComboBox()
        self.resolution_combo.addItems(["HD 2K", "HD 1080", "HD 720", "VGA"])
        self.resolution_combo.setCurrentText("HD 720")  # 默认选择HD 720
        self.resolution_combo.currentTextChanged.connect(self.on_resolution_changed)
        resolution_layout.addWidget(self.resolution_combo)
        camera_layout.addLayout(resolution_layout)
        
        # 当前分辨率显示
        self.current_resolution_label = QLabel("当前分辨率: HD 720")
        self.current_resolution_label.setAlignment(Qt.AlignCenter)
        self.current_resolution_label.setStyleSheet("font-size: 12px; color: #666; padding: 3px;")
        camera_layout.addWidget(self.current_resolution_label)
        
        # 自动聚焦设置
        focus_layout = QHBoxLayout()
        focus_layout.addWidget(QLabel("自动聚焦:"))
        self.auto_focus_checkbox = QCheckBox("启用")
        self.auto_focus_checkbox.setChecked(True)  # 默认启用自动聚焦
        self.auto_focus_checkbox.toggled.connect(self.toggle_auto_focus)
        focus_layout.addWidget(self.auto_focus_checkbox)
        camera_layout.addLayout(focus_layout)
        
        # 手动聚焦按钮
        self.manual_focus_btn = QPushButton("手动聚焦")
        self.manual_focus_btn.clicked.connect(self.trigger_manual_focus)
        self.manual_focus_btn.setStyleSheet("QPushButton { background-color: #2196F3; color: white; font-weight: bold; padding: 5px; }")
        camera_layout.addWidget(self.manual_focus_btn)
        
        control_layout.addWidget(camera_group)
        
        # 曝光控制组
        exposure_group = QGroupBox("曝光控制")
        exposure_layout = QVBoxLayout(exposure_group)
        
        # 曝光值显示
        self.exposure_label = QLabel("当前曝光: 0")
        self.exposure_label.setAlignment(Qt.AlignCenter)
        self.exposure_label.setStyleSheet("font-size: 14px; font-weight: bold;")
        exposure_layout.addWidget(self.exposure_label)
        
        # 曝光滑块
        self.exposure_slider = QSlider(Qt.Horizontal)
        self.exposure_slider.setMinimum(0)
        self.exposure_slider.setMaximum(100)
        self.exposure_slider.setValue(50)
        self.exposure_slider.setTickPosition(QSlider.TicksBelow)
        self.exposure_slider.setTickInterval(10)
        self.exposure_slider.valueChanged.connect(self.on_exposure_slider_changed)
        exposure_layout.addWidget(self.exposure_slider)
        
        # 快速调节按钮
        button_layout = QHBoxLayout()
        
        self.decrease_btn = QPushButton("减少曝光")
        self.decrease_btn.clicked.connect(self.decrease_exposure)
        button_layout.addWidget(self.decrease_btn)
        
        self.increase_btn = QPushButton("增加曝光")
        self.increase_btn.clicked.connect(self.increase_exposure)
        button_layout.addWidget(self.increase_btn)
        
        exposure_layout.addLayout(button_layout)
        
        # 重置按钮
        self.reset_btn = QPushButton("重置曝光")
        self.reset_btn.clicked.connect(self.reset_exposure)
        exposure_layout.addWidget(self.reset_btn)
        
        control_layout.addWidget(exposure_group)
        
        # 光斑检测控制组
        blob_group = QGroupBox("光斑检测")
        blob_layout = QVBoxLayout(blob_group)
        
        self.blob_enable_checkbox = QCheckBox("启用光斑检测")
        self.blob_enable_checkbox.setChecked(True)
        self.blob_enable_checkbox.toggled.connect(self.toggle_blob_detection)
        blob_layout.addWidget(self.blob_enable_checkbox)
        
        # 检测参数调节
        param_layout = QHBoxLayout()
        param_layout.addWidget(QLabel("最小面积:"))
        self.min_area_spinbox = QSpinBox()
        self.min_area_spinbox.setMinimum(10)
        self.min_area_spinbox.setMaximum(1000)
        self.min_area_spinbox.setValue(5)
        self.min_area_spinbox.valueChanged.connect(self.update_blob_detector_params)
        param_layout.addWidget(self.min_area_spinbox)
        blob_layout.addLayout(param_layout)
        
        param_layout2 = QHBoxLayout()
        param_layout2.addWidget(QLabel("最大面积:"))
        self.max_area_spinbox = QSpinBox()
        self.max_area_spinbox.setMinimum(100)
        self.max_area_spinbox.setMaximum(20000)
        self.max_area_spinbox.setValue(5000)
        self.max_area_spinbox.valueChanged.connect(self.update_blob_detector_params)
        param_layout2.addWidget(self.max_area_spinbox)
        blob_layout.addLayout(param_layout2)
        
        # 二值化阈值调节
        threshold_layout = QHBoxLayout()
        threshold_layout.addWidget(QLabel("二值化阈值:"))
        self.threshold_spinbox = QSpinBox()
        self.threshold_spinbox.setMinimum(50)
        self.threshold_spinbox.setMaximum(255)
        self.threshold_spinbox.setValue(240)
        self.threshold_spinbox.valueChanged.connect(self.update_blob_detector_params)
        threshold_layout.addWidget(self.threshold_spinbox)
        blob_layout.addLayout(threshold_layout)
        
        control_layout.addWidget(blob_group)
        
        # 图片保存组
        save_group = QGroupBox("图片保存")
        save_layout = QVBoxLayout(save_group)
        
        self.save_btn = QPushButton("保存当前图片")
        self.save_btn.clicked.connect(self.save_current_images)
        self.save_btn.setStyleSheet("QPushButton { background-color: #4CAF50; color: white; font-weight: bold; padding: 8px; }")
        save_layout.addWidget(self.save_btn)
        
        self.save_info_label = QLabel("图片和配置将保存到 img/ 文件夹")
        self.save_info_label.setAlignment(Qt.AlignCenter)
        self.save_info_label.setStyleSheet("color: gray; font-size: 10px;")
        save_layout.addWidget(self.save_info_label)
        
        control_layout.addWidget(save_group)
        

        

        

        

        
        # 添加弹性空间
        control_layout.addStretch()
        
        top_layout.addWidget(control_widget)
        
        # 将上半部分添加到主布局
        main_layout.addLayout(top_layout)
        
        # 将下半部分添加到主布局
        main_layout.addLayout(bottom_layout)
        
        # 设置布局比例
        main_layout.setStretch(0, 3)  # 上半部分（图像）占3份
        main_layout.setStretch(1, 1)  # 下半部分（三维坐标）占1份
    
    def init_camera(self):
        """初始化相机"""
        try:
            # 检查ZED SDK版本
            print(f"ZED SDK版本: {sl.Camera.get_sdk_version()}")
            
            # 检查可用相机数量
            available_cameras = sl.Camera.get_device_list()
            print(f"检测到 {len(available_cameras)} 个ZED相机")
            
            if len(available_cameras) == 0:
                QMessageBox.critical(self, "错误", "未检测到ZED相机，请检查USB连接")
                return
            
            # 显示相机信息
            for i, camera in enumerate(available_cameras):
                print(f"相机 {i+1}: {camera.camera_model} (SN: {camera.serial_number})")
            
            # 创建相机对象
            self.zed = sl.Camera()
            
            # 设置初始化参数 - 使用界面选择的分辨率
            init_params = sl.InitParameters()
            
            # 根据界面选择设置分辨率
            resolution_map = {
                "HD 2K": sl.RESOLUTION.HD2K,
                "HD 1080": sl.RESOLUTION.HD1080,
                "HD 720": sl.RESOLUTION.HD720,
                "VGA": sl.RESOLUTION.VGA
            }
            
            current_resolution = self.resolution_combo.currentText()
            if current_resolution in resolution_map:
                init_params.camera_resolution = resolution_map[current_resolution]
                print(f"✓ 设置相机分辨率: {current_resolution}")
            else:
                init_params.camera_resolution = sl.RESOLUTION.VGA
                print(f"⚠ 使用默认分辨率: VGA")
            
            # 打开相机（带重试机制）
            max_retries = 3
            for attempt in range(max_retries):
                try:
                    err = self.zed.open(init_params)
                    if err == sl.ERROR_CODE.SUCCESS:
                        break
                    else:
                        print(f"尝试 {attempt + 1}/{max_retries} 失败，错误代码: {err}")
                        if attempt < max_retries - 1:
                            print("等待2秒后重试...")
                            time.sleep(2)
                        else:
                            # 最后一次尝试失败，尝试使用默认参数
                            print("尝试使用默认参数重新初始化...")
                            try:
                                default_params = sl.InitParameters()
                                err = self.zed.open(default_params)
                                if err == sl.ERROR_CODE.SUCCESS:
                                    print("✓ 使用默认参数初始化成功")
                                    break
                                else:
                                    error_msg = f"无法打开 ZED 相机，错误代码: {err}"
                                    print(f"✗ {error_msg}")
                                    QMessageBox.critical(self, "错误", error_msg)
                                    return
                            except Exception as default_e:
                                error_msg = f"相机初始化失败: {default_e}"
                                print(f"✗ {error_msg}")
                                QMessageBox.critical(self, "错误", error_msg)
                                return
                except Exception as e:
                    print(f"尝试 {attempt + 1}/{max_retries} 出现异常: {e}")
                    if attempt < max_retries - 1:
                        print("等待2秒后重试...")
                        time.sleep(2)
                    else:
                        error_msg = f"相机初始化异常: {e}"
                        print(f"✗ {error_msg}")
                        QMessageBox.critical(self, "错误", error_msg)
                        return
            
            # 安全地获取相机信息
            try:
                camera_info = self.zed.get_camera_information()
                print(f"✓ 相机型号: {camera_info.camera_model}")
                print(f"✓ 相机序列号: {camera_info.serial_number}")
                
                # 安全地获取分辨率信息
                try:
                    real_width = camera_info.camera_configuration.resolution.width
                    real_height = camera_info.camera_configuration.resolution.height
                    print(f"✓ 真实分辨率: {real_width} x {real_height}")
                    
                    # 更新界面显示的真实分辨率
                    self.current_resolution_label.setText(f"当前分辨率: {real_width} × {real_height}")
                except:
                    print("✓ 分辨率: 信息不可用")
                
                # 安全地获取帧率信息
                try:
                    print(f"✓ 帧率: {camera_info.camera_configuration.fps} FPS")
                except:
                    print("✓ 帧率: 信息不可用")
                
                # 安全地获取传感器分辨率（如果存在）
                try:
                    if hasattr(camera_info.camera_configuration, 'sensor_resolution'):
                        print(f"✓ 传感器分辨率: {camera_info.camera_configuration.sensor_resolution.width} x {camera_info.camera_configuration.sensor_resolution.height}")
                    else:
                        print("✓ 传感器分辨率: 信息不可用")
                except:
                    print("✓ 传感器分辨率: 信息不可用")
            except Exception as e:
                print(f"⚠ 获取相机信息失败: {e}")
                print("✓ 相机初始化成功（但无法获取详细信息）")
            
            print("✓ 相机初始化成功")
            print("  光斑检测: 已启用")
            
            # 创建并启动相机线程
            self.camera_thread = ZEDCameraThread(self.zed, self)
            self.camera_thread.left_image_ready.connect(self.update_left_image)
            self.camera_thread.right_image_ready.connect(self.update_right_image)
            self.camera_thread.error_occurred.connect(self.handle_error)
            self.camera_thread.blob_detection_ready.connect(self.on_blob_detection_ready)
            
            # 启动线程
            self.camera_thread.start()
            
            # 创建并启动三维坐标计算线程
            self.coordinate_thread = ThreeDCoordinateThread()
            self.coordinate_thread.coordinate_ready.connect(self.update_coordinate_display)
            self.coordinate_thread.start()
            
            # 应用保存的曝光设置
            self.apply_saved_exposure()
            
            # 测试相机是否正常工作
            print("正在测试相机图像捕获...")
            test_image = sl.Mat()
            runtime_params = sl.RuntimeParameters()
            
            # 尝试捕获一帧图像
            if self.zed.grab(runtime_params) == sl.ERROR_CODE.SUCCESS:
                self.zed.retrieve_image(test_image, sl.VIEW.LEFT)
                print(f"✓ 相机测试成功，图像尺寸: {test_image.get_width()} x {test_image.get_height()}")
            else:
                print("⚠ 相机测试失败，但继续运行...")
            
        except Exception as e:
            QMessageBox.critical(self, "错误", f"初始化相机失败: {e}")
    
    def apply_saved_exposure(self):
        """应用保存的曝光设置"""
        try:
            if hasattr(self, 'camera_thread') and self.camera_thread:
                # 获取当前滑块的值
                saved_exposure = self.exposure_slider.value()
                print(f"正在应用保存的曝光设置: {saved_exposure}")
                
                # 应用曝光设置
                success = self.camera_thread.set_exposure(saved_exposure)
                if success:
                    print(f"✓ 曝光设置应用成功: {saved_exposure}")
                    # 更新显示
                    self.update_exposure_display(saved_exposure)
                else:
                    print(f"⚠ 曝光设置应用失败，尝试获取当前真实曝光值")
                    # 尝试获取当前真实曝光值
                    real_exposure = self.camera_thread.get_current_exposure()
                    if real_exposure != saved_exposure:
                        print(f"真实曝光值: {real_exposure}，与保存值不同: {saved_exposure}")
                        # 更新滑块和显示为真实值
                        self.exposure_slider.blockSignals(True)
                        self.exposure_slider.setValue(real_exposure)
                        self.exposure_slider.blockSignals(False)
                        self.update_exposure_display(real_exposure)
        except Exception as e:
            print(f"⚠ 应用曝光设置时出错: {e}")
    
    def sync_exposure_value(self):
        """同步真实曝光值"""
        try:
            if hasattr(self, 'camera_thread') and self.camera_thread:
                # 获取真实曝光值
                real_exposure = self.camera_thread.get_current_exposure()
                current_slider_value = self.exposure_slider.value()
                
                # 如果真实值与滑块值不同，更新显示
                if real_exposure != current_slider_value:
                    print(f"曝光值不同步 - 滑块: {current_slider_value}, 真实: {real_exposure}")
                    # 更新滑块值（不触发信号）
                    self.exposure_slider.blockSignals(True)
                    self.exposure_slider.setValue(real_exposure)
                    self.exposure_slider.blockSignals(False)
                    # 更新显示
                    self.update_exposure_display(real_exposure)
        except Exception as e:
            print(f"同步曝光值失败: {e}")
    
    def update_left_image(self, image_data):
        """更新左眼图像显示"""
        try:
            # 解包图像数据
            display_image, original_image = image_data
            
            # 检查图像数据有效性
            if display_image is None or original_image is None:
                return
                
            if len(display_image.shape) != 3 or display_image.shape[2] != 3:
                return
            
            # 保存当前图像用于保存功能
            self.current_left_image = original_image.copy()
            
            # 保存原始图像数据到可点击标签
            self.left_image_label.setOriginalImage(original_image, "左眼相机 - 原尺寸图像")
            
            # 转换为QImage (BGR格式)
            height, width, channel = display_image.shape
            bytes_per_line = 3 * width
            
            # 确保图像数据类型正确
            if display_image.dtype != np.uint8:
                display_image = display_image.astype(np.uint8)
            
            # 将BGR转换为RGB格式用于QImage显示
            rgb_image = cv2.cvtColor(display_image, cv2.COLOR_BGR2RGB)
            q_image = QImage(rgb_image.data, width, height, bytes_per_line, QImage.Format_RGB888)
            
            # 转换为QPixmap并显示
            pixmap = QPixmap.fromImage(q_image)
            
            # 根据标签大小缩放图像，保持宽高比
            scaled_pixmap = pixmap.scaled(
                self.left_image_label.size(), 
                Qt.KeepAspectRatio, 
                Qt.SmoothTransformation
            )
            self.left_image_label.setPixmap(scaled_pixmap)
            
            # 更新分辨率显示
            self.current_resolution_label.setText(f"当前分辨率: {width} × {height}")
            
        except Exception as e:
            print(f"更新左眼图像失败: {e}")
    
    def update_right_image(self, image_data):
        """更新右眼图像显示"""
        try:
            # 解包图像数据
            display_image, original_image = image_data
            
            # 检查图像数据有效性
            if display_image is None or original_image is None:
                return
                
            if len(display_image.shape) != 3 or display_image.shape[2] != 3:
                return
            
            # 保存当前图像用于保存功能
            self.current_right_image = original_image.copy()
            
            # 保存原始图像数据到可点击标签
            self.right_image_label.setOriginalImage(original_image, "右眼相机 - 原尺寸图像")
            
            # 转换为QImage (BGR格式)
            height, width, channel = display_image.shape
            bytes_per_line = 3 * width
            
            # 确保图像数据类型正确
            if display_image.dtype != np.uint8:
                display_image = display_image.astype(np.uint8)
            
            # 将BGR转换为RGB格式用于QImage显示
            rgb_image = cv2.cvtColor(display_image, cv2.COLOR_BGR2RGB)
            q_image = QImage(rgb_image.data, width, height, bytes_per_line, QImage.Format_RGB888)
            
            # 转换为QPixmap并显示
            pixmap = QPixmap.fromImage(q_image)
            
            # 根据标签大小缩放图像，保持宽高比
            scaled_pixmap = pixmap.scaled(
                self.right_image_label.size(), 
                Qt.KeepAspectRatio, 
                Qt.SmoothTransformation
            )
            self.right_image_label.setPixmap(scaled_pixmap)
            
        except Exception as e:
            print(f"更新右眼图像失败: {e}")
    

    

    


    

    

    

    
    def save_current_images(self):
        """保存当前左右相机图片"""
        try:
            # 检查是否有图像数据
            if self.current_left_image is None or self.current_right_image is None:
                QMessageBox.warning(self, "警告", "没有可保存的图像数据")
                return
            
            # 创建保存文件夹
            if not os.path.exists(self.img_folder):
                os.makedirs(self.img_folder)
            
            # 生成文件名
            left_filename = os.path.join(self.img_folder, f"left{self.img_counter}.jpg")
            right_filename = os.path.join(self.img_folder, f"right{self.img_counter}.jpg")
            
            # 保存图片
            cv2.imwrite(left_filename, self.current_left_image)
            cv2.imwrite(right_filename, self.current_right_image)
            

            
            # 更新计数器
            self.img_counter += 1
            
            # 显示成功消息
            QMessageBox.information(self, "保存成功", 
                                  f"图片已保存到 {self.img_folder}/ 文件夹\n"
                                  f"左眼: left{self.img_counter-1}.jpg\n"
                                  f"右眼: right{self.img_counter-1}.jpg")
            
        except Exception as e:
            error_msg = f"保存图片失败: {e}"
            QMessageBox.critical(self, "保存失败", error_msg)
    

    

    

    

    

            # 更新左眼图像显示
            if left_image is not None:
                self.update_image_display(left_image, self.left_image_label)
            
            # 更新右眼图像显示
            if right_image is not None:
                self.update_image_display(right_image, self.right_image_label)
            
            # 处理三维坐标数据
            if isinstance(result_data, dict):
                simple_result = result_data.get('simple')
                if simple_result is not None:
                    print(f"✓ 图像已更新，简单算法三维坐标: X={simple_result[0]:.1f}mm, Y={simple_result[1]:.1f}mm, Z={simple_result[2]:.1f}mm")
            else:
                print(f"✓ 图像已更新，三维坐标: X={result_data[0]:.1f}mm, Y={result_data[1]:.1f}mm, Z={result_data[2]:.1f}mm")
            
        except Exception as e:
            print(f"更新图像显示失败: {e}")
            import traceback
            traceback.print_exc()
    
    def update_image_display(self, image, label):
        """更新图像显示"""
        try:
            # 检查图像数据有效性
            if image is None:
                return
                
            if len(image.shape) != 3 or image.shape[2] != 3:
                return
            
            # 确保图像数据类型正确
            if image.dtype != np.uint8:
                image = image.astype(np.uint8)
            
            # 将BGR转换为RGB格式用于QImage显示
            rgb_image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
            
            # 转换为QImage
            height, width, channel = rgb_image.shape
            bytes_per_line = 3 * width
            q_image = QImage(rgb_image.data, width, height, bytes_per_line, QImage.Format_RGB888)
            
            # 转换为QPixmap并显示
            pixmap = QPixmap.fromImage(q_image)
            label.setPixmap(pixmap.scaled(
                label.size(), 
                Qt.KeepAspectRatio, 
                Qt.SmoothTransformation
            ))
            
        except Exception as e:
            print(f"更新图像显示失败: {e}")
    
    def on_blob_detection_ready(self, left_centroids, right_centroids, left_contours, right_contours):
        """处理光斑检测结果"""
        try:
            # 保存光斑检测结果
            self.left_centroids = left_centroids
            self.right_centroids = right_centroids
            self.left_contours = left_contours
            self.right_contours = right_contours
            
            # 发送到三维坐标计算线程
            if self.coordinate_thread:
                self.coordinate_thread.update_centroids(left_centroids, right_centroids)
                self.coordinate_thread.update_contours(left_contours, right_contours)
                
        except Exception as e:
            print(f"处理光斑检测结果失败: {e}")
    
    def update_coordinate_display(self, result_data):
        """更新三维坐标显示"""
        try:
            # 检查结果数据类型
            if isinstance(result_data, dict):
                # 只显示OpenCV算法结果
                opencv_result = result_data.get('opencv')
                
                # 检查是否有光斑检测结果
                has_blobs = (len(self.left_centroids) > 0 or len(self.right_centroids) > 0)
                
                # 更新OpenCV算法结果
                if opencv_result is not None:
                    opencv_text = f"X: {opencv_result[0]:.2f} mm    Y: {opencv_result[1]:.2f} mm    Z: {opencv_result[2]:.2f} mm"
                    self.opencv_coord_label.setText(opencv_text)
                    print(f"✓ 三维坐标显示: {opencv_text}")
                elif has_blobs:
                    # 有光斑但没有计算出三维坐标
                    self.opencv_coord_label.setText("光斑已检测，正在计算三维坐标...")
                    print(f"⚠ 光斑已检测但三维坐标计算失败 - 左: {len(self.left_centroids)} 个, 右: {len(self.right_centroids)} 个")
                else:
                    # 没有光斑
                    self.opencv_coord_label.setText("等待检测光斑...")
                    print("等待检测光斑...")
                
            else:
                # 兼容旧格式，单个结果
                if hasattr(self, 'coord_display_label'):
                    current_coord_text = f"X: {result_data[0]:.1f} mm (右)\nY: {result_data[1]:.1f} mm (下)\nZ: {result_data[2]:.1f} mm (前)"
                    self.coord_display_label.setText(current_coord_text)
            
        except Exception as e:
            print(f"更新三维坐标显示失败: {e}")
    
    def handle_error(self, error_message):
        """处理错误"""
        pass
    
    def on_exposure_slider_changed(self, value):
        """曝光滑块变化处理"""
        try:
            if self.camera_thread:
                success = self.camera_thread.set_exposure(value)
                if success:
                    # 更新显示
                    self.update_exposure_display(value)
                    print(f"✓ 曝光滑块设置成功: {value}")
                else:
                    print(f"⚠ 曝光滑块设置失败: {value}")
            # 自动保存配置
            self.save_config()
        except Exception as e:
            print(f"曝光滑块变化处理失败: {e}")
    

    
    def increase_exposure(self):
        """增加曝光"""
        if self.camera_thread:
            current_value = self.exposure_slider.value()
            new_value = min(self.exposure_slider.maximum(), current_value + 1)
            self.exposure_slider.setValue(new_value)
    
    def decrease_exposure(self):
        """减少曝光"""
        if self.camera_thread:
            current_value = self.exposure_slider.value()
            new_value = max(self.exposure_slider.minimum(), current_value - 1)
            self.exposure_slider.setValue(new_value)
    
    def reset_exposure(self):
        """重置曝光"""
        if self.camera_thread:
            default_value = (self.exposure_slider.minimum() + self.exposure_slider.maximum()) // 2
            self.exposure_slider.setValue(default_value)
    
    def update_exposure_display(self, exposure_value):
        """更新曝光显示"""
        try:
            # 确保曝光值是整数
            exposure_value = int(exposure_value)
            self.exposure_label.setText(f"当前曝光: {exposure_value}")
            
            # 同时更新滑块的值（如果不同步的话）
            if self.exposure_slider.value() != exposure_value:
                self.exposure_slider.blockSignals(True)  # 阻止信号触发
                self.exposure_slider.setValue(exposure_value)
                self.exposure_slider.blockSignals(False)  # 恢复信号
                
            print(f"✓ 曝光显示已更新: {exposure_value}")
        except Exception as e:
            print(f"更新曝光显示失败: {e}")
    
    def toggle_blob_detection(self, enabled):
        """切换光斑检测开关"""
        if self.camera_thread:
            self.camera_thread.blob_enabled = enabled
        # 自动保存配置
        self.save_config()
    
    def update_blob_detector_params(self):
        """更新光斑检测器参数"""
        if self.camera_thread and self.camera_thread.blob_detector:
            self.camera_thread.blob_detector.min_area = self.min_area_spinbox.value()
            self.camera_thread.blob_detector.max_area = self.max_area_spinbox.value()
            self.camera_thread.blob_detector.threshold_value = self.threshold_spinbox.value()
            print(f"✓ 光斑检测参数已更新 - 最小面积: {self.min_area_spinbox.value()}, 最大面积: {self.max_area_spinbox.value()}, 阈值: {self.threshold_spinbox.value()}")
        # 自动保存配置
        self.save_config()
    
    def toggle_auto_focus(self, enabled):
        """切换自动聚焦开关"""
        if hasattr(self, 'camera_thread') and self.camera_thread:
            success = self.camera_thread.set_auto_focus(enabled)
        # 自动保存配置
        self.save_config()
    
    def trigger_manual_focus(self):
        """触发手动聚焦"""
        if hasattr(self, 'camera_thread') and self.camera_thread:
            # 禁用按钮防止重复点击
            self.manual_focus_btn.setEnabled(False)
            self.manual_focus_btn.setText("聚焦中...")
            
            # 保存当前曝光值
            current_exposure = self.exposure_slider.value()
            print(f"手动聚焦前保存滑块曝光值: {current_exposure}")
            
            # 直接在当前线程中执行聚焦，避免线程管理问题
            try:
                success = self.camera_thread.trigger_manual_focus()
                if success:
                    print("✓ 手动聚焦完成")
                    # 检查曝光值是否被改变
                    new_exposure = self.camera_thread.get_current_exposure()
                    if new_exposure != current_exposure:
                        print(f"曝光值被改变: {current_exposure} -> {new_exposure}，正在恢复...")
                        # 恢复曝光值
                        self.camera_thread.set_exposure(current_exposure)
                        # 更新滑块和显示
                        self.exposure_slider.blockSignals(True)
                        self.exposure_slider.setValue(current_exposure)
                        self.exposure_slider.blockSignals(False)
                        self.update_exposure_display(current_exposure)
                    else:
                        print("✓ 曝光值保持不变")
                else:
                    print("⚠ 手动聚焦失败")
            finally:
                # 恢复按钮状态
                self.manual_focus_btn.setEnabled(True)
                self.manual_focus_btn.setText("手动聚焦")
    
    def on_resolution_changed(self, resolution_text):
        """分辨率设置变化处理"""
        try:
            # 将分辨率文本转换为ZED SDK的枚举值
            resolution_map = {
                "HD 2K": sl.RESOLUTION.HD2K,
                "HD 1080": sl.RESOLUTION.HD1080,
                "HD 720": sl.RESOLUTION.HD720,
                "VGA": sl.RESOLUTION.VGA
            }
            
            if resolution_text in resolution_map:
                new_resolution = resolution_map[resolution_text]
                
                # 更新显示标签
                self.current_resolution_label.setText(f"当前分辨率: {resolution_text}")
                
                # 如果相机已经打开，需要重新初始化
                if hasattr(self, 'zed') and self.zed:
                    self.reinitialize_camera_with_resolution(new_resolution)
                
                # 保存配置
                self.save_config()
        except Exception as e:
            pass
    
    def reinitialize_camera_with_resolution(self, new_resolution):
        """使用新分辨率重新初始化相机"""
        try:
            # 停止当前线程
            if self.camera_thread:
                self.camera_thread.stop()
                self.camera_thread.wait()
            
            # 关闭相机
            if self.zed:
                self.zed.close()
            
            # 重新初始化相机
            init_params = sl.InitParameters()
            init_params.camera_resolution = new_resolution
            
            print(f"正在重新初始化相机，新分辨率: {new_resolution}")
            
            err = self.zed.open(init_params)
            if err != sl.ERROR_CODE.SUCCESS:
                QMessageBox.critical(self, "错误", f"无法使用新分辨率打开相机，错误代码: {err}")
                return
            
            # 获取新的分辨率信息
            try:
                camera_info = self.zed.get_camera_information()
                new_width = camera_info.camera_configuration.resolution.width
                new_height = camera_info.camera_configuration.resolution.height
                print(f"✓ 新分辨率初始化成功: {new_width} x {new_height}")
                
                # 更新界面显示
                resolution_text_map = {
                    sl.RESOLUTION.HD2K: "HD 2K",
                    sl.RESOLUTION.HD1080: "HD 1080", 
                    sl.RESOLUTION.HD720: "HD 720",
                    sl.RESOLUTION.VGA: "VGA"
                }
                if new_resolution in resolution_text_map:
                    self.current_resolution_label.setText(f"当前分辨率: {resolution_text_map[new_resolution]} ({new_width} × {new_height})")
                else:
                    self.current_resolution_label.setText(f"当前分辨率: {new_width} × {new_height}")
            except Exception as e:
                print(f"获取新分辨率信息失败: {e}")
            
            # 重新创建并启动线程
            self.camera_thread = ZEDCameraThread(self.zed, self)
            self.camera_thread.left_image_ready.connect(self.update_left_image)
            self.camera_thread.right_image_ready.connect(self.update_right_image)
            self.camera_thread.error_occurred.connect(self.handle_error)
            self.camera_thread.blob_detection_ready.connect(self.on_blob_detection_ready)
            
            # 启动线程
            self.camera_thread.start()
            
        except Exception as e:
            pass
    

    
    def save_config(self):
        """保存配置文件"""
        try:
            config = {
                'resolution': self.resolution_combo.currentText(),
                'exposure': {
                    'min': self.exposure_slider.minimum(),
                    'max': self.exposure_slider.maximum(),
                    'value': self.exposure_slider.value()
                },
                'blob_detection': {
                    'enabled': self.blob_enable_checkbox.isChecked(),
                    'min_area': self.min_area_spinbox.value(),
                    'max_area': self.max_area_spinbox.value(),
                    'threshold': self.threshold_spinbox.value()
                },
                'focus': {
                    'auto_focus_enabled': self.auto_focus_checkbox.isChecked()
                },
                'save_info': {
                    'img_counter': self.img_counter,
                    'last_save_time': time.strftime("%Y-%m-%d %H:%M:%S")
                }
            }
            
            config_file = os.path.join(os.path.dirname(os.path.abspath(__file__)), "zed_camera_config.json")
            with open(config_file, 'w', encoding='utf-8') as f:
                json.dump(config, f, indent=2, ensure_ascii=False)
            
            print(f"✓ 配置已保存到: {config_file}")
        except Exception as e:
            print(f"✗ 保存配置失败: {e}")
            import traceback
            traceback.print_exc()
    
    def load_config(self):
        """加载配置文件"""
        try:
            config_file = os.path.join(os.path.dirname(os.path.abspath(__file__)), "zed_camera_config.json")
            if os.path.exists(config_file):
                print(f"正在加载配置文件: {config_file}")
                with open(config_file, 'r', encoding='utf-8') as f:
                    config = json.load(f)
                
                # 加载分辨率设置
                if 'resolution' in config:
                    self.resolution_combo.setCurrentText(config['resolution'])
                    print(f"✓ 加载分辨率设置: {config['resolution']}")
                
                # 加载曝光设置
                if 'exposure' in config:
                    exposure_config = config['exposure']
                    if 'min' in exposure_config:
                        self.exposure_slider.setMinimum(exposure_config['min'])
                    if 'max' in exposure_config:
                        self.exposure_slider.setMaximum(exposure_config['max'])
                    if 'value' in exposure_config:
                        self.exposure_slider.setValue(exposure_config['value'])
                        print(f"✓ 加载曝光设置: {exposure_config['value']}")
                
                # 加载光斑检测设置
                if 'blob_detection' in config:
                    blob_config = config['blob_detection']
                    if 'enabled' in blob_config:
                        self.blob_enable_checkbox.setChecked(blob_config['enabled'])
                    if 'min_area' in blob_config:
                        self.min_area_spinbox.setValue(blob_config['min_area'])
                    if 'max_area' in blob_config:
                        self.max_area_spinbox.setValue(blob_config['max_area'])
                    if 'threshold' in blob_config:
                        self.threshold_spinbox.setValue(blob_config['threshold'])
                    print(f"✓ 加载光斑检测设置: 启用={blob_config.get('enabled', False)}, 面积范围={blob_config.get('min_area', 50)}-{blob_config.get('max_area', 5000)}, 阈值={blob_config.get('threshold', 240)}")
                
                # 加载聚焦设置
                if 'focus' in config:
                    focus_config = config['focus']
                    if 'auto_focus_enabled' in focus_config:
                        self.auto_focus_checkbox.setChecked(focus_config['auto_focus_enabled'])
                        print(f"✓ 加载聚焦设置: 自动聚焦={focus_config['auto_focus_enabled']}")
                
                # 加载保存信息
                if 'save_info' in config:
                    save_config = config['save_info']
                    if 'img_counter' in save_config:
                        self.img_counter = save_config['img_counter']
                        print(f"✓ 加载图片计数器: {self.img_counter}")
                    if 'last_save_time' in save_config:
                        print(f"✓ 上次保存时间: {save_config['last_save_time']}")
                
                print("✓ 配置文件加载完成")
            else:
                print("⚠ 配置文件不存在，将使用默认设置")
        except Exception as e:
            print(f"✗ 加载配置文件失败: {e}")
            import traceback
            traceback.print_exc()
    
    def closeEvent(self, event):
        """窗口关闭事件"""
        try:
            # 停止定时器
            if hasattr(self, 'exposure_sync_timer'):
                self.exposure_sync_timer.stop()
            
            # 保存所有配置到JSON文件
            print("正在保存配置到JSON文件...")
            self.save_config()
            print("✓ 配置保存成功")
        except Exception as e:
            print(f"⚠ 保存配置失败: {e}")
        
        # 停止三维坐标计算线程
        if self.coordinate_thread:
            self.coordinate_thread.stop()
            self.coordinate_thread.wait()
        
        # 停止相机线程
        if self.camera_thread:
            self.camera_thread.stop()
            self.camera_thread.wait()
        
        # 关闭相机
        if self.zed:
            self.zed.close()
        
        event.accept()


def main():
    """主函数"""
    
    app = QApplication(sys.argv)
    
    try:
        window = ZEDExposureControlGUI()
        window.show()
        sys.exit(app.exec_())
    except Exception as e:
        QMessageBox.critical(None, "错误", f"程序运行错误: {e}")


if __name__ == "__main__":
    main()
